%% A* algorithm in matlab
%% --------------a sketch of Astar algorithm---------------------- 
% OPEN = priority queue containing START
% CLOSED = empty set
% while lowest rank in OPEN is not the GOAL:
% current = remove lowest rank item from OPEN
% add current to CLOSED
% for neighbors of current:
%   cost = g(current) + movementcost(current, neighbor)
%   if neighbor in OPEN and cost less than g(neighbor):
%     remove neighbor from OPEN, because new path is better
%   if neighbor in CLOSED and cost less than g(neighbor): **
%     remove neighbor from CLOSED
%   if neighbor not in OPEN and neighbor not in CLOSED:
%     set g(neighbor) to cost
%     add neighbor to OPEN
%     set priority queue rank to g(neighbor) + h(neighbor)
%     set neighbor's parent to current

%reconstruct reverse path from goal to start
%by following parent pointers
%% -------------end of introduction  -----------------------------

function [path_x, path_y] = Astar(map,M,N,start,endpoint,Ymax,Ymin,Xmin)
%% ------------------initialize-----------------------------------

opentable = [];   % open table
closetable = [];  % close table
interval = 0.00003;
     
manhattan = @(x,y) max(abs(x/M-y/M), abs(mod(x,M)-mod(y,M)))*2;
%max(abs(x/M-y/M), abs(mod(x,M)-mod(y,M)));
%abs(x/M-y/M) + abs(mod(x,M)-mod(y,M));
%sqrt((x/M-y/M)^2 +(mod(x,M)-mod(y,M))^2);      % a bunch of estimation function

node(M,N) = struct( ...                                       % 
                'state',[], ...                               % state: 1: traversable, 9: untraversable
                'f',[], ...
                'h',[], ...
                'g',[], ...
                'parent',[]);

%% ---------initialize the information of the map-----------------
for i = 1:M
    for j = 1:N
        node(i,j).state = map(i,j);
        node(i,j).f = 0;
        node(i,j).h = 0;
        node(i,j).g = 0;
        node(i,j).parent = 0;
    end
end
% ���start���end��

%% ---------------------Astar-------------------------
opentable = [opentable,start];                                  % 
done = 0;
%% -------------------------main loop -----------------------------------------------------
while ~done    
    node_f = [];                                                % 
    for i = opentable
        node_f = [node_f,node(i).f];
    end
    [current_f,current_i] = min(node_f);                        % 
                                                                % 
    current_index = opentable(current_i);                       % 

    opentable(current_i) = [];                                  % 
    closetable = [closetable,current_index];                    % 

    adjacent_diag_node = adjacent_diag(current_index);          % 
    adjacent_straight_node = adjacent_straight(current_index);  % 
    for i = [adjacent_diag_node,adjacent_straight_node]         % 
        if (node(i).state ==9) || any(find(closetable == i))     %

        elseif ~any(find(opentable == i))                       % 
            opentable = [opentable,i];                          % 
            node(i).parent = current_index;                     % 
                                                                % 
            if any(find(adjacent_diag_node == i))               % 
                node(i).g = 1.414 + node(node(i).parent).g;     % 
            else                                                % 
                node(i).g = 1 + node(node(i).parent).g;         % 
            end
            node(i).h = manhattan(i,endpoint);                   % 
            node(i).f = node(i).h + node(i).g;                  % 
        else                                                    % 
            if any(find(adjacent_diag_node == i))               % 
                if 1.414 + node(node(i).parent).g < node(i).g
                    node(i).parent = current_index;
                    node(i).g = 1.414 + node(node(i).parent).g;
                    node(i).f = node(i).h + node(i).g;
                end
            else                                                % 
                if 1 + node(node(i).parent).g < node(i).g
                    node(i).parent = current_index;
                    node(i).g = 1 + node(node(i).parent).g;
                    node(i).f = node(i).h + node(i).g;
                end
            end
        end
    end

    if any(find(closetable == endpoint)) || ~any(opentable)      % 
        done = 1;
    end
end
%% ----------------------find the path ---------------------------------------------------
done = 0;
   path_x = [];
   path_y = [];
while ~done

    temp = node(endpoint).parent;

    if temp == start
        done = 1;
    else
        yOff = mod(temp,M);
        if  yOff == 0
            path_y = [Ymax, path_y];
        else
            path_y = [Ymin + sym(yOff *interval), path_y];
        end
        path_x = [Xmin + sym(floor(temp/M) * interval), path_x];

        endpoint = temp;
    end
end

% path_x = [];
% path_y = [];
% 
% for i = 1: length(path)
%     if mod(path(i),M) == 0
%         path_y = [path_y, 20];
%     else
%         path_y = [path_y, mod(path(i),M)];
%     end
%     path_x = [path_x, ceil(path(i)/M)];
% end
% 
% startHDL = plot(path_x(1),path_y(1), 'x');
% set(startHDL, 'XData',path_x(1), 'YData',path_y(1));
% startTxt = text(path_x(1),path_y(1), sprintf('Start'), ...
%     'Color',[0.2 0.2 0.2], 'FontSize',8, ...
%     'HorizontalAlignment','left', 'VerticalAlignment','top');
% set(startTxt, 'Position',[path_x(1) path_y(1)], ...
%         'String',sprintf('Start'));
% 
% endHDL = plot(path_x(end),path_y(end), 'd');
% set(endHDL, 'XData',path_x(end), 'YData',path_y(end));
% endTxt = text(path_x(end),path_y(end), sprintf('Goal'), ...
%     'Color',[0.2 0.2 0.2], 'FontSize',8, ...
%     'HorizontalAlignment','left', 'VerticalAlignment','top');
% set(endTxt, 'Position',[path_x(end) path_y(end)], ...
%         'String',sprintf('Goal'));
% 
% pathHDL = plot(path_x, path_y, '-b');
% set(pathHDL, 'XData',path_x, 'YData',path_x);

%% ----------------function adjacent_diag-------------------------------------
function adjacent = adjacent_diag(x)            % 
    if x ==1                                    % 
        adjacent = x+M+1;
    elseif x==M                                 % 
        adjacent = x+M-1;
    elseif x==(N-1)*M + 1                       % 
        adjacent = x-M+1;
    elseif x== M*N                              % 
        adjacent = x-M-1;
    elseif floor(x/M) ==0                       % 
        adjacent = [x+M-1,x+M+1];
    elseif (floor(x/M) ==N - 1)                 % 
        adjacent = [x-M-1,x-M+1];
    elseif (mod(x,M) ==1)                       % 
        adjacent = [x-M+1,x+M+1];               
    elseif mod(x,M) == 0                        % 
        adjacent = [x-M-1,x+M-1];
    else                                        % 
        adjacent = [x-M-1,x-M+1,x+M-1,x+M+1]; 
    end
end
%% ----------------function adjacent_diag----------------------------
function adjacent = adjacent_straight(x)  % 
    if x ==1
        adjacent = [x+1,x+M];
    elseif x==M
        adjacent = [x-1,x+M];
    elseif x==(N-1)*M + 1
        adjacent = [x-M,x+1];
    elseif x== M*N
        adjacent = [x-M,x-1];
    elseif floor(x/M) ==0
        adjacent = [x-1,x+1,x+M];
    elseif floor(x/M) ==N - 1
        adjacent = [x-1,x+1,x-M];
    elseif mod(x,M) ==1
        adjacent = [x-M,x+M,x+1];
    elseif mod(x,M) == 0
        adjacent = [x-M,x+M,x-1];
    else
        adjacent = [x-1,x+1,x-M,x+M]; 
    end
end
%% -------------end of function adjacent_straight -------------------------
end

%% ------------------------------the end-------------------